#include<points_cloud_algorithm.h>
#include<Mesh/lib_cell_iterator.h>
#define Matrix4x4 Viewer_Matrix4x4_
#define quote lib_cell_quote

#define MAX2(a,b) a>b?a:b
#define MIN2(a,b) a<b?a:b
void Points_Cloud_Piexel_init(Points_Cloud_Piexel* pcp)
{
    //pcp->loc[0]=0;pcp->loc[1]=0;pcp->loc[2]=0;
    //pcp->r=0;
    pcp->n=NULL;
    pcp->prop=NULL;
}

void points_cloud_piexel_free(Points_Cloud_Piexel* pcp)
{
    if(pcp==NULL)
    {
        return ;
    }
    free_node(pcp->n);
    free(pcp);
}
void Points_Cloud_Image_init(Points_Cloud_Image* pci)
{
    pci->a1=0;pci->a2=0;pci->a3=0;
    pci->data=NULL;
    pci->loc[0]=0;pci->loc[1]=0;pci->loc[2]=0;
    pci->r=1.0/1000.0;


}

void points_cloud_image_free(Points_Cloud_Image* pci)
{
    if(pci==NULL)
    {
        return;
    }

    if(pci->data!=NULL)
    {
        for(int i=0;i<pci->a1;i++)
        {
            for(int j=0;j<pci->a2;j++)
            {
                for(int k=0;k<pci->a3;k++)
                {
                    free_node(pci->data[i][j][k].n);
                }
                free(pci->data[i][j]);
            }
            free(pci->data[i]);
        } 
    }
    free(pci->data);
    free(pci);
}

static Points_Cloud_Piexel*** create_points_cloud_piexels(int a1,int a2,int a3)
{
    Points_Cloud_Piexel*** re=(Points_Cloud_Piexel***)malloc(sizeof(Points_Cloud_Piexel**)*a1);
    for(int i=0;i<a1;i++)
    {
        re[i]=(Points_Cloud_Piexel**)malloc(sizeof(Points_Cloud_Piexel*)*a2);
        for(int j=0;j<a2;j++)
        {
            re[i][j]=(Points_Cloud_Piexel*)malloc(sizeof(Points_Cloud_Piexel)*a3);

            for(int k=0;k<a3;k++)
            {   
                Points_Cloud_Piexel_init(&re[i][j][k]);

            } 
        }
    } 
    return re;
} 


/*
static void free_points_cloud_piexel(Points_Cloud_Piexel*** pcp)
{

}*/
//using namespace std;


void points_cloud_analyzer_init_(Points_Cloud_Analyzer* pca)
{
    pca->xs[0]=0;pca->xs[1]=0;
    pca->ys[0]=0;pca->ys[1]=0;
    pca->zs[0]=0;pca->zs[1]=0;
    pca->centroid[0]=0;pca->centroid[1]=0;pca->centroid[2]=0;

    //***************
    pca->tree=NULL;
    //****************
    pca->median_distance=0;
    //pca->dense_density=0;
   // pca->sparse_density=0;
}
void points_cloud_analyzer_free(Points_Cloud_Analyzer*pca)
{
    if(pca==NULL)
    {
        return ;
    }
    free(pca);
}

Points_Cloud_Analyzer* points_cloud_analyze_points_from_mesh(Mesh* m)
{
    if(m->num_v(m)<=0)
    {
        return NULL;
    } 

    Points_Cloud_Analyzer*pca=(Points_Cloud_Analyzer*)malloc(sizeof(Points_Cloud_Analyzer));
    points_cloud_analyzer_init_(pca);
    template_v* v=NULL;
    v=(quote(m->v_begin(m)));
    

    pca->xs[0]=v->point[0];pca->xs[1]=v->point[0];
    pca->ys[0]=v->point[1];pca->ys[1]=v->point[1];
    pca->zs[0]=v->point[2];pca->zs[1]=v->point[2];
    int sum=0;
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        v=(template_v*)(quote(vit));
        pca->centroid[0]+=v->point[0];pca->centroid[1]+=v->point[1];pca->centroid[2]+=v->point[2];
        pca->xs[0]=MIN2(pca->xs[0],v->point[0]);
        pca->xs[1]=MAX2(pca->xs[1],v->point[0]);
        // if(v->point[0]<pca->xs[0])
        // {
        //     pca->xs[0]=v->point[0];
        // }
        // else if(v->point[0]>pca->xs[1])
        // {
        //     pca->xs[1]=v->point[0];
        // }

        pca->ys[0]=MIN2(pca->ys[0],v->point[1]);
        pca->ys[1]=MAX2(pca->ys[1],v->point[1]);
        // if(v->point[1]<pca->ys[0])
        // {
        //     pca->ys[0]=v->point[1];
        // }
        // else if(v->point[1]>pca->ys[1])
        // {
        //     pca->ys[1]=v->point[1];
        // }
        pca->zs[0]=MIN2(pca->zs[0],v->point[2]);
        pca->zs[1]=MAX2(pca->zs[1],v->point[2]);

        // if(v->point[2]<pca->zs[0])
        // {
        //     pca->zs[0]=v->point[2];
        // }
        // else if(v->point[2]>pca->zs[1])
        // {
        //     pca->zs[1]=v->point[2];
        // }
        sum++;

    } 
    pca->centroid[0]/=(double)sum; pca->centroid[1]/=(double)sum; pca->centroid[2]/=(double)sum; 

    pca->tree=(OC_Node*)malloc(sizeof(OC_Node));
    OC_Node_init(pca->tree);
    OC_Node* ocn=pca->tree;
    ocn->loc_min[0]=pca->xs[0];ocn->loc_min[1]=pca->ys[0];ocn->loc_min[2]=pca->zs[0];
    double max=pca->xs[1]-pca->xs[0];
    if(pca->ys[1]-pca->ys[0]>max)
    {
        max=pca->ys[1]-pca->ys[0];
    }
    if(pca->zs[1]-pca->zs[0]>max)
    {
        max=pca->zs[1]-pca->zs[0];
    }

    ocn->loc_max[0]=max+ocn->loc_min[0];ocn->loc_max[1]=max+ocn->loc_min[1];ocn->loc_max[2]=max+ocn->loc_min[2];
  //  printf("%lf %lf %lf %lf %lf %lf\n",ocn->loc_min[0],ocn->loc_min[1],ocn->loc_min[2],ocn->loc_max[0],ocn->loc_max[1],ocn->loc_max[2] );
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        ocn->value=node_overlying(ocn->value,quote(vit));
    }
   // printf("max:%lf\n",max );

    int num1=1,num2=1,depth=0;
    do
     {
        num1=num2;
        num2=oc_node_divide_all_leaves(ocn);
    
    //    printf("%d \n",num2 );
        depth++;
     } while(num2>1.5*num1); 
    /*for(int i=0;i<11;i++)
    {
        oc_node_divide_all_leaves(ocn); 
        printf("%d\n",oc_node_voxel_size(ocn) );
    }*/
    //oc_node_divide_all_leaves(ocn);
    
    
    
    pca->median_distance=max/pow(2,depth-1); 
    //printf("sum:%d %d %d %lf \n",sum,m->num_v(m) ,oc_node_voxel_size(ocn),max/pow(2,depth-1));
    oc_node_free(ocn);
    pca->tree=NULL;



    return pca;
  
}

Mesh* points_cloud_simplify_points_from_mesh(Mesh* m,double fac,std::map<int,int> &mp1,std::map<int,Node*> &mp2)
{

    if(fac<1||m->num_v(m)<=0)
    {
        return NULL;
    } 
    int num1=(int)(m->num_v(m)/fac);
    Points_Cloud_Analyzer *pca=NULL;
    //points_cloud_analyzer_init_(&pca); 
    pca=points_cloud_analyze_points_from_mesh(m);

    OC_Node* ocn=(OC_Node*)malloc(sizeof(OC_Node));
    OC_Node_init(ocn);
    ocn->loc_min[0]=pca->xs[0];ocn->loc_min[1]=pca->ys[0];ocn->loc_min[2]=pca->zs[0];
    double max=pca->xs[1]-pca->xs[0];
    if(pca->ys[1]-pca->ys[0]>max)
    {
        max=pca->ys[1]-pca->ys[0];
    }
    if(pca->zs[1]-pca->zs[0]>max)
    {
        max=pca->zs[1]-pca->zs[0];
    }

    ocn->loc_max[0]=max+ocn->loc_min[0];ocn->loc_max[1]=max+ocn->loc_min[1];ocn->loc_max[2]=max+ocn->loc_min[2];
    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        ocn->value=node_overlying(ocn->value,quote(vit));
    }

    while(oc_node_divide_all_leaves(ocn)<num1)
    {
    } 

//    printf("%d\n",oc_node_voxel_size(ocn) );
    Mesh*re=(Mesh*)malloc(sizeof(Mesh));
    Mesh_init(re); 
    double p[3]={0,0,0};
    template_v* v1=NULL,*v2=NULL;
    int size=0;
    Node* n=NULL;
    for(auto oit=octree_begin(ocn);oit!=octree_end(ocn);oit++)
    {
        if(oit.it->value!=NULL)
        {

            v2=re->create_vertexv(re,p,3);
            p[0]=0;p[1]=0;p[2]=0;size=0;
            n=NULL;
            for(Node* nit=oit.it->value;nit!=NULL;nit=(Node*)(nit->Next))
            {
                v1=(template_v*)(nit->value);
                p[0]+=v1->point[0];p[1]+=v1->point[1];p[2]+=v1->point[2];
                size++;

             //   mp1.insert(&mp1,v1->id,);
                mp1[v1->id]=v2->id;
                n=node_overlying(n,v1);  
            } 
            p[0]/=(double)(size);p[1]/=(double)(size);p[2]/=(double)(size);
            v2->point[0]=p[0];v2->point[1]=p[1];v2->point[2]=p[2];
            mp2[v2->id]=n;
            
        }

    }
    oc_node_free(ocn);
    points_cloud_analyzer_free(pca);

    return re;

}
void points_cloud_analyze_points(Node* n,Points_Cloud_Analyzer*pca)
{
    if(n==NULL)
    {
        return ;
    }
    Node* nit=n;
    template_v* v=NULL;v=(template_v*)(nit->value); 
    pca->xs[0]=v->point[0];pca->xs[1]=v->point[0];
    pca->ys[0]=v->point[1];pca->ys[1]=v->point[1];  
    pca->zs[0]=v->point[2];pca->zs[1]=v->point[2];
    int sum=0;
    for(;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v=(template_v*)(nit->value);
        pca->centroid[0]+=v->point[0];pca->centroid[1]+=v->point[1];pca->centroid[2]+=v->point[2];
        pca->xs[0]=MIN2(pca->xs[0],v->point[0]);
        pca->xs[1]=MAX2(pca->xs[1],v->point[0]); 

        // if(v->point[0]<pca->xs[0])
        // {
        //     pca->xs[0]=v->point[0];
        // }
        // else if(v->point[0]>pca->xs[1])
        // {
        //     pca->xs[1]=v->point[0];
        // }
        pca->ys[0]=MIN2(pca->ys[0],v->point[1]);
        pca->ys[1]=MAX2(pca->ys[1],v->point[1]);
        // if(v->point[1]<pca->ys[0])
        // {
        //     pca->ys[0]=v->point[1];
        // }
        // else if(v->point[1]>pca->ys[1])
        // {
        //     pca->ys[1]=v->point[1];
        // }

        pca->zs[0]=MIN2(pca->zs[0],v->point[2]);
        pca->zs[1]=MAX2(pca->zs[1],v->point[2]); 
        // if(v->point[2]<pca->zs[0])
        // {
        //     pca->zs[0]=v->point[2];
        // }
        // else if(v->point[2]>pca->zs[1])
        // {
        //     pca->zs[1]=v->point[2];
        // }


        sum++;
    }
    pca->centroid[0]/=(double)sum; pca->centroid[1]/=(double)sum; pca->centroid[2]/=(double)sum; 

    pca->tree=(OC_Node*)malloc(sizeof(OC_Node));
    OC_Node_init(pca->tree);
    OC_Node* ocn=pca->tree;
    for(nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        ocn->value=node_overlying(ocn->value,nit->value);
    }


/*
    Points_Cloud_Image* pci=(Points_Cloud_Image*)malloc(sizeof(Points_Cloud_Image));
    points_cloud_image_init(pci);
    double len=pca->xs[1]-pca->xs[0];
    if(pca->ys[1]-pca->ys[0]>len)
    {
        len=pca->ys[1]-pca->ys[0];
    }
    if(pca->zs[1]-pca->zs[0]>len)
    {
        len=pca->zs[1]-pca->zs[0];
    }
*/  
/*    double num_cuts=1000;

    pci->r=len/num_cuts;
    pci->a1=(int)((pca->xs[1]-pca->xs[0])/pci->r)+1;
    pci->a2=(int)((pca->ys[1]-pca->ys[0])/pci->r)+1;
    pci->a3=(int)((pca->zs[1]-pca->zs[0])/pci->r)+1;
    printf("%d %d %d sum:%d\n",(int)(-1.8),pci->a2,pci->a3,sum );

    pci->data=create_points_cloud_piexels(pci->a1,pci->a2,pci->a3);

    pci->loc[0]=pca->xs[0];pci->loc[1]=pca->ys[0];pci->loc[2]=pca->zs[0];

    sum=0;
    int i=0,j=0,k=0;
    for(auto kit=kd_tree_begin(kn);kit!=kd_tree_end(kn);kit++)
    {
        i=(int)((kit.it->value->point[0]-pci->loc[0])/pci->r);
        j=(int)((kit.it->value->point[1]-pci->loc[1])/pci->r);
        k=(int)((kit.it->value->point[2]-pci->loc[2])/pci->r);
        
        Node* n=pci->data[i][j][k].n;
        
        pci->data[i][j][k].n=node_overlying(n,kit.it->value);

        //kit.it->value->
    } 
    sum=0;*/

  /*  for(i=0;i<pci->a1;i++)
    {
        for(j=0;j<pci->a2;j++)
        {
            for(k=0;k<pci->a3;k++)
            {
                if(pci->)
                {

                }
            }

        }

    }*/
   // printf("%d\n",sum );
   // points_cloud_image_free(pci);
//    pca->pci=pci;
}

//根据点云构造图像，需要将点云放在单位立方体内

static Node* points_cloud_dyeing_and_get_connected_points(Node*nvs,KD_Node*kn,double dis,void* p)
{
  //  printf("heree\n");
    Node* re=NULL;
    for(Node* nit=nvs;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_v* v=(template_v*)(nit->value);
        v->prop=p;

        Node* U=kd_tree_find_nearest_sphere(v,dis,kn);

        for(Node*nnit=U;nnit!=NULL;nnit=(Node*)(nnit->Next))
        {
            template_v* v1=(template_v*)(nnit->value);
            if(v1->prop==NULL)
            {
                v1->prop=p;
                re=node_overlying(re,v1);
            } 

        }

        free_node(U);
    }

    return re;
}
//@parameter vb:the start vertex

//@parameter kn: the kd tree included all vertices

//@parameter dis: the distance to decide open set


Node* points_cloud_find_connected_vertices(template_v* vb,KD_Node* kn,double dis)
{
    Node* re=NULL;
    if(vb==NULL)
    {
        return re;
    }
    void* p=malloc(sizeof(int));

    Node* n1=NULL,*n2=NULL;
    n1=node_overlying(n1,vb);
    vb->prop=p;
    while(n1!=NULL)
    {
       n2=points_cloud_dyeing_and_get_connected_points(n1,kn,dis,p);
       free_node(n1);
       n1=n2; 
    }

    for(auto kit=kd_tree_begin(kn);kit!=kd_tree_end(kn);kit++)
    {
        if(kit.it->value->prop==p)
        {
            re=node_overlying(re,kit.it->value);
            kit.it->value->prop=NULL; 
        } 
    }
    free(p);
    return re; 
}
Node* points_cloud_classify_points_from_mesh(Mesh* m,double dis)
{
    if(m->num_v(m)<=0)
    {
        return NULL;
    }

    Node* re=NULL;
    template_v* vb=NULL,*v=NULL;
    KD_Node* kn=NULL;
    Node* n1=NULL;
    Int_RB_Tree * mp=(Int_RB_Tree*)malloc(sizeof(Int_RB_Tree));
    int_rb_tree_init(mp); 

    for(auto vit=m->v_begin(m);vit!=m->v_end(m);vit++)
    {
        mp->insert(mp,quote(vit)->id,quote(vit));
        //mp[((template_v*)(quote(vit)))->id]=(template_v*)(quote(vit));
    }

    do{
        vb=(template_v*)(mp->begin(mp).second);
        //vb=mp.begin()->second;

        free_kdnode(kn);kn=NULL;
        kn=create_kd_tree_from_RBTree(mp);
        n1=NULL;
        n1=points_cloud_find_connected_vertices(vb, kn,dis);
        re=node_overlying(re,n1);
        for(Node* nit=n1;nit!=NULL;nit=(Node*)(nit->Next))
        {
            v=(template_v*)(nit->value);
            mp->erase(mp,v->id);
            //mp.erase(v->id);
        }       
    }while(mp->size>0);

    int_rb_tree_free(mp);
    return re;  

}
static double* points_cloud_compute_out_product(double *p1,double* p2)
{
    double * re=(double*)malloc(sizeof(double)*3);

    re[0]=p1[1]*p2[2]-p1[2]*p2[1];
    re[1]=-p1[0]*p2[2]+p1[2]*p2[0];
    re[2]=p1[0]*p2[1]-p1[1]*p2[0];
    return re;
}
static double points_cloud_compute_inner_product(double *p1,double*p2,int dim)
{
    double re=0;
    for(int i=0;i<dim;i++)
    {
        re+=p1[i]*p2[i];
    }

    return re;
}
static double points_cloud_compute_cos_angle(double* p1,double* p2,int dim)
{
    double norm1=0,norm2=0;

    for(int i=0;i<dim;i++)
    {
        norm1+=p1[i]*p1[i];
        norm2+=p2[i]*p2[i];
    }
    norm1=SAFE_SQRT(norm1);norm2=SAFE_SQRT(norm2);

    double re=0;
    for(int i=0;i<dim;i++)
    {
        re+=p1[i]*p2[i];
    } 
    re=re/(norm1*norm2);    
    return  re;
}

static double *points_cloud_compute_normal_vector_from_3points(double * p1,double *p2,double *p3)
{
    if(p1==p2||p1==p3||p2==p3)
    {
        double* re=(double*)malloc(sizeof(double)*3);
        memset(re,0,sizeof(double)*3);

        return re;

    }
    double q1[3]={p2[0]-p1[0],p2[1]-p1[1],p2[2]-p1[2]};    
    double q2[3]={p3[0]-p1[0],p3[1]-p1[1],p3[2]-p1[2]};

    return points_cloud_compute_out_product(q1,q2);    
}
static double *points_cloud_compute_normal_vector_from_cell(template_c* c)
{
    return points_cloud_compute_normal_vector_from_3points(c->vertices[0]->point,c->vertices[1]->point,c->vertices[2]->point);    
}
static bool points_cloud_vertex_is_in_triangle(double *p,double * p1,double * p2,double *p3,double * n)
{

    Matrix4x4* mat=(Matrix4x4*)malloc(sizeof(Matrix4x4));
    Matrix4x4_init_float(mat);

    float* data=(float*)(mat->data);
    data[0*4+0]=p2[0]-p1[0]; data[1*4+0]=p2[1]-p1[1];data[2*4+0]=p2[2]-p1[2];
    data[0*4+1]=p3[0]-p1[0]; data[1*4+1]=p3[1]-p1[1];data[2*4+1]=p3[2]-p1[2];
    data[0*4+2]=n[0]; data[1*4+2]=n[1];data[2*4+2]=n[2];
    double y[3]={p[0]-n[0],p[1]-n[1],p[2]-n[2]};

    Matrix4x4* mati=mat->inverse(mat);
    double x[3]={0,0,0};
    if(mati==NULL)
    {
        printf("mati is null\n");
         Matrix4x4_free(mat);
         return false;
    } 
    else
    {
        data=(float*)(mati->data);
        for(int i=0;i<3;i++)
        {
            for(int j=0;j<3;j++)
            {
                x[i]+=data[i*4+j]*y[j];
            }
        }
        Matrix4x4_free(mati); 
    }
    Matrix4x4_free(mat);

    if(x[0]<=0||x[1]<=0||x[0]>=1||x[1]>=1)
    {
        return false;
    }

    return true;
}

static Node* points_cloud_get_all_connected_same_dir_cells(Mesh* m,template_c* c,double * dir)
{

    Node* re=NULL,*temp_n=NULL,*temp_n1=NULL,*temp_n2=NULL;
    temp_n=node_overlying(temp_n,c);
    re=node_overlying(re,c);

    void* mark=malloc(sizeof(int));
    c->prop=mark;
    template_c* c1=NULL,*c3=NULL;
    
    while(temp_n!=NULL)
    {
        temp_n1=NULL;
        for(Node* nit=temp_n;nit!=NULL;nit=(Node*)(nit->Next))
        {
            c1=(template_c*)(nit->value);
            for(auto chfit=m->chf_begin(m,*c1);chfit!=m->chf_end(m,*c1);chfit++)
            {
                c3=m->s_opposite_halfface(quote(chfit))->cell;
                if(c3==NULL)
                {
                    continue;
                }
                if(c3->prop==NULL)
                {
                    c3->prop=mark; 
                    temp_n2=node_overlying(temp_n2,c3);

                    double * dir1=points_cloud_compute_normal_vector_from_cell(c3); 
                    double is_same_dir=points_cloud_compute_inner_product(dir,dir1,3);
                    if(is_same_dir>0)
                    {
                        temp_n1=node_overlying(temp_n1,c3);
                        re=node_overlying(re,c3);
                    }
                    free(dir1);
                }
            } 
        }
        free_node(temp_n);
        temp_n=temp_n1;
    }
    for(Node* nit=temp_n2;nit!=NULL;nit=(Node*)(nit->Next))
    {
        c1=(template_c*)(nit->value);
        c1->prop=NULL;
    }
    free(mark);
   return re; 
}

static Node* points_cloud_get_all_connected_same_dir_cells_from_sets(Mesh* m,template_c* c,double * dir,std::map<int,template_c*>&mp1)
{
    Node* re=NULL,*temp_n=NULL,*temp_n1=NULL,*temp_n2=NULL;
    temp_n=node_overlying(temp_n,c);
    re=node_overlying(re,c);
    void* mark=malloc(sizeof(int));
    c->prop=mark;
    temp_n2=node_overlying(temp_n2,c);
    template_c* c1=NULL,*c3=NULL;
    //template_hf* hf1=NULL;
    while(temp_n!=NULL)
    {
        temp_n1=NULL;
        for(Node* nit=temp_n;nit!=NULL;nit=(Node*)(nit->Next))
        {
            c1=(template_c*)(nit->value);
            for(auto chfit=m->chf_begin(m,*c1);chfit!=m->chf_end(m,*c1);chfit++)
            {
                c3=m->s_opposite_halfface(quote(chfit))->cell;
                if(c3==NULL)
                {
                    continue;
                }
                if(c3->prop==NULL)
                {
                    c3->prop=mark; 
                    temp_n2=node_overlying(temp_n2,c3);
                    auto iter=mp1.find(c3->id);

                    if(iter==mp1.end())
                    {
                        continue;
                    }
                    double * dir1=points_cloud_compute_normal_vector_from_cell(c3); 
                    double is_same_dir=points_cloud_compute_inner_product(dir,dir1,3);
                    if(is_same_dir>0)
                    {
                        temp_n1=node_overlying(temp_n1,c3);
                        re=node_overlying(re,c3);
                    }
                    free(dir1);
                }
            } 
        }
        free_node(temp_n);
        temp_n=temp_n1;
    }
    for(Node*nit=temp_n2;nit!=NULL;nit=(Node*)(nit->Next))
    {
        c1=(template_c*)(nit->value);
        c1->prop=NULL;
    }
    free_node(temp_n2);
    free(mark);
    return re; 
}
//需要face的prop,请提前清除prop为NULL
static Node* points_cloud_get_connect_same_dir_halffaces_from_cells_and_halffaces(Mesh* m,template_hf*hf,template_v*v,double* dir,Node* cells,Node* UHF)
{
    if(hf->cell!=NULL)
    {
        printf("hf is not boundary\n");
        return NULL;
    }
    Node* re=NULL;
    template_v* v1=NULL,*v2=NULL,*v3=NULL;
    template_hf* hf1=m->s_opposite_halfface(hf),*hf2=NULL;
    v1=hf1->vertices[0];v2=hf1->vertices[1];
    printf("%d %d %d\n",v1->id,v2->id,v->id );
    if(v!=v1&&v!=v2)
    {
        double* dir3=points_cloud_compute_normal_vector_from_3points(v2->point,v1->point,v->point);
        if(points_cloud_compute_inner_product(dir,dir3,3)<0)
        {
            return NULL;
        } 
        free(dir3);
    } 
 
    re=node_overlying(re,hf);
    Node* NM=NULL;

    template_f*f1=hf1->face;
    void* mark=malloc(sizeof(int));
    f1->prop=mark;
    NM=node_overlying(NM,f1);
    while(v1!=NULL||v2!=NULL)
    {
        if(v1!=NULL)
        {
            v3=v1;
            v1=NULL;
            for(auto vfit=m->vf_begin(m,*v3);vfit!=m->vf_end(m,*v3);vfit++)
            {
                f1=quote(vfit);

                if(f1->prop==NULL)
                {
                    f1->prop=mark;
                    NM=node_overlying(NM,f1);
                    hf1=NULL;
                    if(f1->halffaces[0]->cell==NULL)
                    {
                        hf1=f1->halffaces[0];
                    }
                    else if(f1->halffaces[1]->cell==NULL)
                    {
                        hf1=f1->halffaces[1];
                    }

                    if(hf1!=NULL&&node_find(UHF,hf1)!=NULL)
                    {
                        hf2=m->s_opposite_halfface(hf1);
                    
                        if(node_find(cells,hf2->cell)!=NULL)
                        {
                            double* dir2=points_cloud_compute_normal_vector_from_3points(hf2->vertices[1]->point,hf2->vertices[0]->point,v->point);
                            if(points_cloud_compute_inner_product(dir,dir2,3)>=0)
                            {
                                re=node_overlying(re,hf1);

                                if(hf2->vertices[0]==v3)
                                {
                                    v1=hf2->vertices[1];
                                }
                                else
                                {
                                    v1=hf2->vertices[0];
                                }
                            }
                            free(dir2);
                            break;
                        }       
                    }
                }   
            }
        }

        if(v2!=NULL)
        {
            v3=v2;
            v2=NULL;

            for(auto vfit=m->vf_begin(m,*v3);vfit!=m->vf_end(m,*v3);vfit++)
            {
                f1=quote(vfit);
                if(f1->prop==NULL)
                {
                    f1->prop=mark;
                    NM=node_overlying(NM,f1);
                    hf1=NULL;
                    if(f1->halffaces[0]->cell==NULL)
                    {
                        hf1=f1->halffaces[0];
                    }
                    else if(f1->halffaces[1]->cell==NULL)
                    {
                        hf1=f1->halffaces[1];
                    }
                    if(hf1!=NULL&&node_find(UHF,hf1)!=NULL)
                    {
                        hf2=m->s_opposite_halfface(hf1);
                        if(node_find(cells,hf2->cell)!=NULL)
                        {
                            double* dir2=points_cloud_compute_normal_vector_from_3points(hf2->vertices[1]->point,hf2->vertices[0]->point,v->point);

                            if(points_cloud_compute_inner_product(dir,dir2,3)>=0)
                            {
                                re=node_overlying(re,hf1);

                                if(hf2->vertices[0]==v3)
                                {
                                    v2=hf2->vertices[1];
                                }
                                else
                                {
                                    v2=hf2->vertices[0];
                                }
                            }
                            free(dir2);
                            break;
                        }       
                    }
                }  
            }
        }
    }
    for(Node* nit=NM;nit!=NULL;nit=(Node*)(nit->Next))
    {
        f1=(template_f*)(nit->value);
        f1->prop=NULL;
    }
    free_node(NM);
    free(mark);
    return re;

}
static bool points_cloud_vertex_is_overlap_with_cells(template_v* v,std::map<int,template_c*>&mp1,double *dir)
{
    template_c* c=NULL;
    for(auto citer=mp1.begin();citer!=mp1.end();citer++)
    {
        c=citer->second;
        if(c->vertices[0]==v||c->vertices[1]==v||c->vertices[2]==v)
        {
            continue;
        }
        double * dir1=points_cloud_compute_normal_vector_from_cell(c);
        if(points_cloud_compute_inner_product(dir1,dir,3)>0)
        {
            if(points_cloud_vertex_is_in_triangle(v->point,c->vertices[0]->point,c->vertices[1]->point,c->vertices[2]->point,dir))
            {
                free(dir1);
                return true;
            }
        }
        free(dir1); 
    } 
    return false;
}

static void Points_Cloud_Construct_Info_init_(Points_Cloud_Construct_Info* pcci)
{
    pcci->hfs=NULL;
    pcci->hf=NULL;
    pcci->angle_difference=0;
    pcci->tohf_dis=0;
    pcci->prop=NULL;
}
//需要用到face的prop,请提前清除prop为NULL。
//需要用到cell的prop,请提前清除prop为NULL.
//请确保m是二维流形
Points_Cloud_Construct_Info* points_cloud_construct_mesh_from_one_point(Mesh*m,KD_Node* kn,template_v*v,double dis)
{

    if(!m->vertex_is_boundary(m,*v))
    {
       // printf("不是边界点\n");
        return NULL;
    }
    Node* U =kd_tree_find_nearest_sphere(v,dis,kn);
    Node* U1=NULL;
    template_v* v1=NULL;
    for(Node* nit=U;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v1=(template_v*)(nit->value);
        if(v1->cells!=NULL&&m->vertex_is_boundary(m,*v1))
        {
            //printf("%d\n",v1->id );
            U1=node_overlying(U1,v1);
        }
    } 
    free_node(U);U=NULL;
    if(U1==NULL)
    {
        return NULL;
    }
    //UM用来储存mark点
    Node* UHF=NULL,*UM=NULL;
    void* mark=malloc(sizeof(int));
    template_f* f1=NULL;
    template_hf* hf1=NULL;
    for(Node* nit=U1;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v1=(template_v*)(nit->value);
        for(auto vfit=m->vf_begin(m,*v1);vfit!=m->vf_end(m,*v1);vfit++)
        {
            f1=quote(vfit);
            if(f1->prop==NULL)
            {
                hf1=NULL;

                if(f1->halffaces[0]->cell==NULL)
                {
                    hf1=f1->halffaces[0];
                }
                else if(f1->halffaces[1]->cell==NULL)
                {
                    hf1=f1->halffaces[1];
                }
                if(hf1!=NULL)
                {

                    f1->prop=mark;
                    UM=node_overlying(UM,f1);
                    if(f1->vertices[0]==v1)
                    {
                        if(node_find(U1,f1->vertices[1])==NULL)
                        {
                            continue;
                        }
                    }
                    else
                    {
                        if(node_find(U1,f1->vertices[0])==NULL)
                        {
                            continue;
                        }
                    }
                //    printf("f:%d %d\n",f1->vertices[0]->id,f1->vertices[1]->id );
                    UHF=node_overlying(UHF,hf1);
                } 
            }
        }
    }
    for(Node*nit=UM;nit!=NULL;nit=(Node*)(nit->Next))
    {
        f1=(template_f*)(nit->value);
        f1->prop=NULL;
    }

    free(mark);
    free_node(UM);UM=NULL;
    if(UHF==NULL)
    {
        free_node(UHF);UHF=NULL;
        return NULL;
    }
   // printf("size UHF:%d\n",node_size(UHF) );
    
    template_hf* nearest_hf=NULL,*temp_hf=NULL;
    double nearest_dis=0,temp_dis=0;
    
    std::map<int,template_c*>mp1;
    for(Node* nit=U1;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v1=(template_v*)(nit->value);
        for(auto vcit=m->vc_begin(m,*v1);vcit!=m->vc_end(m,*v1);vcit++)
        {
            mp1[quote(vcit)->id]=quote(vcit);
        }
    }
    Points_Cloud_Construct_Info* re=NULL;

    if(v->cells==NULL)
    {
        //如果v是孤立点的话
        //求最近的半边
        nearest_hf=(template_hf*)(UHF->value);
        double mid_p[3]={0,0,0};
        mid_p[0]=(nearest_hf->face->vertices[0]->point[0]+nearest_hf->face->vertices[1]->point[0])/2.0;
        mid_p[1]=(nearest_hf->face->vertices[0]->point[1]+nearest_hf->face->vertices[1]->point[1])/2.0;
        mid_p[2]=(nearest_hf->face->vertices[0]->point[2]+nearest_hf->face->vertices[1]->point[2])/2.0;

        double a[3]={v->point[0]-mid_p[0],v->point[1]-mid_p[1],v->point[2]-mid_p[2]};
        nearest_dis=SAFE_SQRT(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);

        for(Node* nit=UHF;nit!=NULL;nit=(Node*)(nit->Next))
        {
            temp_hf=(template_hf*)(nit->value);
            mid_p[0]=(temp_hf->face->vertices[0]->point[0]+temp_hf->face->vertices[1]->point[0])/2.0;
            mid_p[1]=(temp_hf->face->vertices[0]->point[1]+temp_hf->face->vertices[1]->point[1])/2.0;
            mid_p[2]=(temp_hf->face->vertices[0]->point[2]+temp_hf->face->vertices[1]->point[2])/2.0;
            a[0]=v->point[0]-mid_p[0];a[1]=v->point[1]-mid_p[1];a[2]=v->point[2]-mid_p[2];
            temp_dis=SAFE_SQRT(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);

            if(temp_dis<nearest_dis)
            {
                nearest_dis=temp_dis;
                nearest_hf=temp_hf;
            }  
        }

        hf1=m->s_opposite_halfface(nearest_hf);
        //printf("near v:%d v:%d\n",hf1->vertices[0]->id,hf1->vertices[1]->id );
        double *dir=points_cloud_compute_normal_vector_from_cell(hf1->cell);

//检查v是否在dir的投影下和mp1重叠,重叠的话终止计算
        if(points_cloud_vertex_is_overlap_with_cells(v,mp1,dir))
        {
            re=NULL;
        }
        else
        {
            //计算和c连通和相同方向的cell集合,他属于mp1
            Node* connect_cells=points_cloud_get_all_connected_same_dir_cells_from_sets(m,hf1->cell,dir,mp1);
            //计算和nearest_hf连同的边界集合，它属于mp1,UHF
            Node* connect_halffaces=points_cloud_get_connect_same_dir_halffaces_from_cells_and_halffaces(m,nearest_hf,v,dir,connect_cells,UHF);

            free_node(connect_cells);
             
            re=(Points_Cloud_Construct_Info*)malloc(sizeof(Points_Cloud_Construct_Info));
            Points_Cloud_Construct_Info_init_(re);
            re->hf=nearest_hf;
            re->hfs=connect_halffaces;
            re->tohf_dis=nearest_dis;
            double *dir1=points_cloud_compute_normal_vector_from_3points(hf1->vertices[1]->point,hf1->vertices[0]->point,v->point);
            re->angle_difference=points_cloud_compute_cos_angle(dir,dir1,3);
            free(dir1);
        }
        free(dir);
    } 
    else
    {
        
        //如果v是边界点的话
        for(Node* nit=v->faces;nit!=NULL;nit=(Node*)(nit->Next))
        {
            f1=(template_f*)(nit->value);
            if(f1->halffaces[0]->cell==NULL)
            {
                nearest_hf=f1->halffaces[1];
                break;
            }
            else if(f1->halffaces[1]->cell==NULL)
            {
                nearest_hf=f1->halffaces[1];
                break;
            }
        }
        hf1=m->s_opposite_halfface(nearest_hf);
        double *dir=points_cloud_compute_normal_vector_from_cell(hf1->cell);

//检查v是否在dir的投影下和mp1重叠,重叠的话终止计算
        if(points_cloud_vertex_is_overlap_with_cells(v,mp1,dir))
        {
            re=NULL;
        }
        else
        {         
            //计算和c连通和相同方向的cell集合,他属于mp1
            Node* connect_cells=points_cloud_get_all_connected_same_dir_cells_from_sets(m,hf1->cell,dir,mp1);
            //计算和nearest_hf连同的边界集合，它属于mp1,UHF
            Node* connect_halffaces=points_cloud_get_connect_same_dir_halffaces_from_cells_and_halffaces(m,nearest_hf,v,dir,connect_cells,UHF);
            if(node_size(connect_halffaces)<2)
            {
                printf("cuowu**********cuowu****************%d\n",node_size(connect_halffaces) );
            }
            free_node(connect_cells);
            re=(Points_Cloud_Construct_Info*)malloc(sizeof(Points_Cloud_Construct_Info));
            Points_Cloud_Construct_Info_init_(re);
            re->hf=nearest_hf;
            re->hfs=connect_halffaces;
            re->angle_difference=0;
        }
        free(dir);
    }
    
    free_node(UHF);
    free_node(U1);
    return re;
}
void points_cloud_construct_mesh_from_points_set(Mesh* m,KD_Node* kn,Node* n,double dis)
{
    printf("********************************\n");
    template_v* v=NULL;
    template_hf*hf=NULL;
    template_v* vs[3]={NULL,NULL,NULL};
    for(Node* nit=n;nit!=NULL;nit=(Node*)(nit->Next))
    {
        v=(template_v*)(nit->value);
        printf("vid:%d\n",v->id );
        auto pcci=points_cloud_construct_mesh_from_one_point(m,kn,v,dis);
        if(pcci!=NULL)
        {
            for(Node* nnit=pcci->hfs;nnit!=NULL;nnit=(Node*)(nnit->Next))
            {
                hf=(template_hf*)(nnit->value);
                hf=m->s_opposite_halfface(hf);
                if(hf->vertices[0]==v||hf->vertices[1]==v)
                {
                    continue;
                } 
                vs[0]=hf->vertices[1];vs[1]=hf->vertices[0];vs[2]=v;
                printf("create cell v:%d %d %d\n",vs[0]->id,vs[1]->id,vs[2]->id );
                 
                m->create_cellv(m,vs,3);
                
            } 
        }
    } 
}

void points_cloud_construct_mesh_from_mesh(Mesh* m,KD_Node*kn,double dis)
{
    m->external_cell_init_(m);
    template_v v1;
    Vertex_init_(&v1);
    v1.point=(double*)malloc(sizeof(double)*3);
    v1.point_size=3;
    for(Node* nit=m->external_cell.halffaces;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_hf* hf=(template_hf*)(nit->value);
        if(hf->cell==NULL)
        { 
            hf=m->s_opposite_halfface(hf);
            for(int i=0;i<3;i++)
            {
                v1.point[i]=(hf->vertices[0]->point[i]+hf->vertices[1]->point[i])/2.0;
              //  printf("%lf ",v1.point[i] );

            }
        //printf("fds%d %d\n",hf->vertices[0]->id,hf->vertices[1]->id ); 
           // printf("\n");
            Node* n1=kd_tree_find_nearest_sphere(&v1,dis,kn);  
            points_cloud_construct_mesh_from_points_set(m,kn,n1,dis);
       
            free_node(n1); 
        }
    }    
    m->external_cell_init_(m);
    for(Node* nit=m->external_cell.halffaces;nit!=NULL;nit=(Node*)(nit->Next))
    {
        template_hf* hf=(template_hf*)(nit->value);
        if(hf->cell==NULL)
        { 
            hf=m->s_opposite_halfface(hf);
            for(int i=0;i<3;i++)
            {
                v1.point[i]=(hf->vertices[0]->point[i]+hf->vertices[1]->point[i])/2.0;
              //  printf("%lf ",v1.point[i] );

            }
        //printf("fds%d %d\n",hf->vertices[0]->id,hf->vertices[1]->id ); 
           // printf("\n");
            Node* n1=kd_tree_find_nearest_sphere(&v1,dis,kn);  
            points_cloud_construct_mesh_from_points_set(m,kn,n1,dis);
       
            free_node(n1); 
        }
    }


    free(v1.point);
}
/*
//输入的mesh包含点集和一个cell,
void points_cloud_construct_mesh_from_mesh(Mesh* m,KD_Node*kn,double dis)
{

    m->external_cell_init_(m);
    for(Node* nit=)
    {

    }
}*/

/*

Node* points_cloud_find_connected_vertices1(template_v* vb,KD_Node* kn,double dis)
{
    Node* re=NULL;
    if(vb==NULL)
    {
        return re;
    }
    
    void* p=malloc(sizeof(int));

    Node* n1=NULL,*n2=NULL;
    n1=node_overlying(n1,vb);
    vb->prop=p;
    while(n1!=NULL)
    {

       n2=points_cloud_dyeing_and_get_connected_points(n1,kn,dis,p);
       free_node(n1);
       n1=n2; 

    }

    for(auto kit=kd_tree_begin(kn);kit!=kd_tree_end(kn);kit++)
    {
       if(kit.it->value->prop==p)
       {
            re=node_overlying(re,kit.it->value);
            kit.it->value->prop=NULL; 
       } 

    }

    free(p);
    return re;    
}*/
#undef Matrix4x4



